import pyb
from pyb import Timer, Pin
import cmath

class Encoder:
    def __init__(self, encoder_freq):
        '''
        入口参数：编码器读取频率
        修改：    
            1.需要自行定义编码器引脚及其相关定时器配置
            2.需要自行修改编码器的分辨率
        # 速度值=(采集到的脉冲数/编码器精度)*轮子周长*控制频率
        
        这里使用的TT编码器电机的参数：
            电机转速：10000rpm
            减速比：1:48
            分辨率：624（编码器精度，转一圈输出的脉冲数）
        '''
        #配置编码器AB相读取模式,利用定时器中断进行读取
        p1_A = Pin("Y1"); p2_A = Pin("Y2");
        tim_A = Timer(8)
        tim_A.init(prescaler=0,period=10000)
        ch1_A = tim_A.channel(1,Timer.ENC_AB,pin=p1_A); ch2_A = tim_A.channel(2,Timer.ENC_AB, pin=p2_A);
        p1_B = Pin("X1"); p2_B = Pin("X2");
        tim_B = Timer(5)
        tim_B.init(prescaler=0,period=10000)
        ch1_B = tim_B.channel(1,Timer.ENC_AB,pin=p1_B); ch2_B = tim_B.channel(2,Timer.ENC_AB, pin=p2_B)
        
        # 初始化常量
        self.pi = cmath.pi # pi的值
        self.encoder_precision = 4*624 #/48 # 编码器精度 = 倍频数*编码器精度（电机驱动线数）*电机减速比        
        self.encoder_freq = encoder_freq
        self.tim_A = tim_A
        self.tim_B = tim_B
        self.encoder_A = 0
        self.encoder_B = 0
        # 定时器中断：固定时间读取编码器数值
        timer_read = Timer(10,freq=self.encoder_freq,callback=self.encoder_cb)

    def encoder_cb(self, encoder):
        '''
        函数功能：通过定时器中断读取编码器A、B计数值，并重置编码器
        '''    
        if self.tim_A.counter() > 5000:
            self.encoder_A = (10000 - self.tim_A.counter())
        else:
            self.encoder_A = self.tim_A.counter()
        if self.tim_B.counter() > 5000:
            self.encoder_B = (10000 - self.tim_B.counter())
        else:
            self.encoder_B = self.tim_B.counter()
        # print("encoder_A:", self.encoder_A, "encoder_B:", self.encoder_B)
        self.tim_A.counter(0)
        self.tim_B.counter(0)

    def get_speed_A(self):
        '''
        函数功能：读取编码器A计数值,将编码器A计数值转化为速度值，同时将编码器值置零
        入口参数：编码器对应定时器
        返回值  ：编码器值
        # 速度值=(采集到的脉冲数/编码器精度)*轮子周长*控制频率
        '''
        speed = (self.encoder_A / self.encoder_precision) * (2*self.pi*0.065) * self.encoder_freq   
        # print("-----------------------speed_A:", speed)
        return speed

    def target_enc_process(self, speed):
        '''
        函数功能：将目标速度值转化为目标编码器值
        入口参数：目标速度值
        返回值  ：目标编码器值
        # 目标编码器值=(目标速度*编码器精度)/(轮子周长*控制频率)
        '''
        enc = (speed*self.encoder_precision) / ( (2*self.pi*0.065) * self.encoder_freq)
        return enc